/*
* mituner_bridge_server.c - Socket server for MiTuner
*
* Copyright 2018 Microsemi Inc. All rights reserved.
* Licensed under the MIT License. See LICENSE.txt in the project root for license information.
*/

#include "mituner_bridge_server.h"

/**********************************************************************
 * Signal Handler for SIGINT
 * ********************************************************************/
void sigintHandler(int sig_num) {
    /* Close the HBI driver */
    HBI_close(gHandle);
    printf("\nServer terminated and HBI driver released\n");
    fflush(stdout);
    exit(EXIT_SUCCESS);
}

/**********************************************************************
 * Store chars in the circular buffer
 * ********************************************************************/
void StoreBuffer(char * const circBuff, unsigned short *writeIndex, const char *buff, unsigned short numBytes) {
    unsigned short tempSize;

    if ((*writeIndex + numBytes) <= C_BUFFER_SZ) {
        memcpy(circBuff + *writeIndex, buff, numBytes);
        *writeIndex += numBytes;
    } else {
        // Loop to get the full number of requested bytes
        tempSize = C_BUFFER_SZ - *writeIndex;
        memcpy(circBuff + *writeIndex, buff, tempSize);
        *writeIndex = 0;
        memcpy(circBuff + *writeIndex, buff + tempSize, numBytes - tempSize);
        *writeIndex += numBytes - tempSize;
    }

    // Update the count
    gCharCount += numBytes;

    if (gCharCount > C_BUFFER_SZ) {
        fprintf(stderr, "StoreBuffer(): Circular buffer overflow\n");
        exit(EXIT_FAILURE);
    }
}

/**********************************************************************
 * Get chars from the circular buffer
 * ********************************************************************/
void GetBuffer(char * const circBuff, unsigned short *readIndex, char *buff, unsigned short numBytes) {
    unsigned short tempSize;

    if ((*readIndex + numBytes) <= C_BUFFER_SZ) {
        memcpy(buff, circBuff + *readIndex, numBytes);
        *readIndex += numBytes;
    } else {
        // Loop to get the full number of requested bytes
        tempSize = C_BUFFER_SZ - *readIndex;
        memcpy(buff, circBuff + *readIndex, tempSize);
        *readIndex = 0;
        memcpy(buff + tempSize, circBuff + *readIndex, numBytes - tempSize);
        *readIndex += numBytes - tempSize;
    }

    // Update the count
    gCharCount -= numBytes;

    if (gCharCount < 0) {
        fprintf(stderr, "GetBuffer(): Circular buffer underflow\n");
        exit(EXIT_FAILURE);
    }
}

/**********************************************************************
 * Wrapper around the HBI_read that return a 16b register value
 * ********************************************************************/
unsigned short HbiReadReg(reg_addr_t reg) {
    hbi_status_t status;
    user_buffer_t buf[2];

    status = HBI_read(gHandle, reg, buf, 2);
    if (status != HBI_STATUS_SUCCESS) {
        fprintf(stderr, "Error - HbiReadReg(0x%03X): (%d)\n", reg, status);
        HBI_close(gHandle);
        exit(EXIT_FAILURE);
    }

    return (((unsigned short)buf[0] << 8) + (unsigned short)buf[1]);
}

/**********************************************************************
 * Wrapper around the HBI_write that return a 16b register value
 * ********************************************************************/
void HbiWriteReg(reg_addr_t reg, unsigned short data) {
    hbi_status_t status;
    user_buffer_t buf[2];

    buf[0] = (data >> 8) & 0x00FF;
    buf[1] = data & 0x00FF;
    status = HBI_write(gHandle, reg, buf, 2);
    if (status != HBI_STATUS_SUCCESS) {
        fprintf(stderr, "Error - HbiWriteReg(0x%03X): (%d)\n", reg, status);
        HBI_close(gHandle);
        exit(EXIT_FAILURE);
    }
}

/**********************************************************************
 * Read a buffer of data and returns a concatenated string
 * ********************************************************************/
void HbiReadBuffS(reg_addr_t reg, char * buffer, unsigned short nbBytes) {
    hbi_status_t status;
    user_buffer_t buf[256];
    unsigned short i;

    status = HBI_read(gHandle, reg, buf, nbBytes);
    if (status != HBI_STATUS_SUCCESS) {
        fprintf(stderr, "Error - HbiReadBuffS(0x%03X): (%d)\n", reg, status);
        HBI_close(gHandle);
        exit(EXIT_FAILURE);
    }

    // Convert the array of bytes in a string
    for (i = 0; i < nbBytes; i++) {
        sprintf(buffer + i * 2, "%02X", buf[i]);
    }
}

/**********************************************************************
 * Write a buffer of data provided as a string
 * ********************************************************************/
void HbiWriteBuffS(reg_addr_t reg, char * buffer, unsigned short nbBytes) {
    hbi_status_t status;
    user_buffer_t buf[256];
    char temp[3] = {0};
    unsigned short i;

    // Convert the string in an array of bytes
    for (i = 0; i < nbBytes; i++) {
        strncpy(temp, buffer + i * 2, 2);
        buf[i] = strtol(temp, NULL, 16);
    }

    status = HBI_write(gHandle, reg, buf, nbBytes);
    if (status != HBI_STATUS_SUCCESS) {
        fprintf(stderr, "Error - HbiWriteReg(0x%03X): (%d)\n", reg, status);
        HBI_close(gHandle);
        exit(EXIT_FAILURE);
    }
}

/**********************************************************************
 * Read a buffer of data and return a concatenated string
 * ********************************************************************/
void HbiHwReadBuffS(unsigned int reg, char * buffer, unsigned short nbBytes) {
    hbi_status_t status;
    user_buffer_t buf[256];
    unsigned short i;

    // Write the address in the indirect register
    buf[0] = (reg >> 24) & 0x000000FF;
    buf[1] = (reg >> 16) & 0x000000FF;
    buf[2] = (reg >> 8) & 0x000000FF;
    buf[3] = reg & 0x000000FF;
    status = HBI_write(gHandle, 0x00C, buf, 4);
    if (status != HBI_STATUS_SUCCESS) {
        fprintf(stderr, "Error - HbiHwReadBuffS(0x00C): (%d)\n", status);
        HBI_close(gHandle);
        exit(EXIT_FAILURE);
    }

    // Read the requested bytes via the page 255
    status = HBI_read(gHandle, 0xFF00 | buf[3], buf, nbBytes);
    if (status != HBI_STATUS_SUCCESS) {
        fprintf(stderr, "Error - HbiHwReadBuffS(0x%04X): (%d)\n", 0xFF00 | buf[3], status);
        HBI_close(gHandle);
        exit(EXIT_FAILURE);
    }

    // Convert the array of bytes in a string
    for (i = 0; i < nbBytes; i++) {
        sprintf(buffer + i * 2, "%02X", buf[i]);
    }
}

/**********************************************************************
 * Write a S3 or S7 record to memory
 * ********************************************************************/
short WriteRecord(record_type recType, unsigned char count, char * record) {
    unsigned short i;
    unsigned int checksum = 0;
    char temp[5];
    user_buffer_t buf[256];
    hbi_status_t status;

    // Convert the string in an array of bytes
    for (i = 0; i < count; i++) {
        strncpy(temp, record + i * 2, 2);
        buf[i] = strtol(temp, NULL, 16);
        checksum += buf[i];
    }

    // Check the checksum
    checksum += count + 1;
    if ((checksum & 0x000000FF) != 0) {
        fprintf(stderr, "Error - WriteRecord(): SRecord checksum error\n");
        HBI_close(gHandle);
        exit(EXIT_FAILURE);
    }

    if (recType == S3) {
        // Write the address in the indirect register
        status = HBI_write(gHandle, 0x00C, buf, 4);
        if (status != HBI_STATUS_SUCCESS) {
            fprintf(stderr, "Error - WriteRecord(0x00C): (%d)\n", status);
            HBI_close(gHandle);
            exit(EXIT_FAILURE);
        }

        // Write the data (excluding the address and the checksum: 5 bytes)
        status = HBI_write(gHandle, 0xFF00 | buf[3], &buf[4], count - 5);
        if (status != HBI_STATUS_SUCCESS) {
            fprintf(stderr, "Error - WriteRecord(0x%04X): (%d)\n", buf[3], status);
            HBI_close(gHandle);
            exit(EXIT_FAILURE);
        }
    } else if (recType == S7) {
        // Write the execution address
        status = HBI_write(gHandle, 0x12C, buf, 4);
        if (status != HBI_STATUS_SUCCESS) {
            fprintf(stderr, "Error - WriteRecord(0x12C): (%d)\n", status);
            HBI_close(gHandle);
            exit(EXIT_FAILURE);
        }

        // Compute the firmware checksum
        status = HBI_set_command(gHandle, HBI_CMD_LOAD_FWR_COMPLETE, NULL);
        if (status != HBI_STATUS_SUCCESS) {
            fprintf(stderr, "Error - WriteRecord(): Firmware checksum error (%d)\n", status);
            return -1;
        }
    }

    return 0;
}

/**********************************************************************
 * Load the firmware via the page 255
 * ********************************************************************/
void LoadFirmware(char * header, char * buffer) {
    unsigned char length, count;
    char *token;
    char *rest = buffer;
    char temp[5];
    record_type recType;
    static char rem[128] = {0};

    while ((token = strtok_r(rest, "\r\n", &rest))) {
        // Check if there's a partial string stored
        if (rem[0] != 0) {
            // A partial record can't be split in more than two segments
            // Concatenate the two pieces
            token = strcat(rem, token);
        }

        length = strlen(token);

        if (length >= 4) {
            if (strncmp(token, "S3", 2) == 0) {
                recType = S3;
            } else if (strncmp(token, "S7", 2) == 0) {
                recType = S7;
            } else {
                recType = NA;
                continue;
            }

            if ((recType == S3) || (recType == S7)) {
                // Get the record size
                strncpy(temp, token + 2, 2);
                count = strtol(temp, NULL, 16);

                // Check if it's a complete record (exclude the first 4 chars, 1 byte = 2 nibbles)
                if (count  == ((length - 4) / 2)) {
                    if (WriteRecord(recType, count, token + 4) != 0) {
                        sprintf(buffer, "ERROR");
                        return;
                    }

                    // Reset the partial string buffer every time we have a full one
                    rem[0] = 0;
                } else if (count > ((length - 4) / 2)) {
                    // Store the partial string for next time
                    strcpy(rem, token);
                } else {
                    fprintf(stderr, "Error - LoadFirmware(): Parsing error\n");
                    sprintf(buffer, "ERROR");
                    return;
                }
            }
        } else {
            // Store the partial string for next time
            strcpy(rem, token);
        }
    }

    sprintf(buffer, "OK");
}

/**********************************************************************
 * Check if the firmware is running
 * ********************************************************************/
unsigned char IsFirmwareRunning(void) {

    if (HbiReadReg(0x028) & 0x8000) {
        return TRUE;
    } else {
        return FALSE;
    }
}

/**********************************************************************
 * Poll the Host Command register (reads 0xFFFF when busy)
 * ********************************************************************/
unsigned char BusySpinWait(void) {
    unsigned short timerMs = 0;

    while ((HbiReadReg(0x032) != 0x0000) && (timerMs < TIMEOUT_MS)) {
        usleep(10000);
        timerMs += 10;
    }

    if (timerMs >= TIMEOUT_MS) {
        fprintf(stderr, "Warning - BusySpinWait(): Command timed out\n");
        return FALSE;
    } else {
        return TRUE;
    }
}

/**********************************************************************
 * Detect the flash
 * ********************************************************************/
unsigned char InitFlash(void) {
    unsigned short status;

    // Detect the flash (Assumes that the firmware is stopped)
    HbiWriteReg(0x032, 0x000B);
    HbiWriteReg(0x006, 0x0001);
    if (BusySpinWait() == FALSE) {
        return FALSE;
    }

    // Check for error
    status = HbiReadReg(0x034) & 0x000F;
    if (status != 0) {
        fprintf(stderr, "Warning - InitFlash(): No flash detected (0x%02X)\n", status);
        return FALSE;
    } else {
        return TRUE;
    }
}

/**********************************************************************
 * Erase the SPI flash
 * ********************************************************************/
void EraseFlash(char * buffer) {
    hbi_status_t status;

    status = HBI_set_command(gHandle, HBI_CMD_ERASE_WHOLE_FLASH, NULL);
    if (status != HBI_STATUS_SUCCESS) {
        fprintf(stderr, "Error - EraseFlash(): Can't erase flash (%d)\n", status);
        sprintf(buffer, "ERROR");
    } else {
        sprintf(buffer, "OK");
    }
}

/**********************************************************************
 * Save firmware to flash
 * ********************************************************************/
void SaveFirmware2Flash(char * buffer) {
    hbi_status_t status;

    if (InitFlash() == FALSE) {
        sprintf(buffer, "ERROR");
        return;
    }

    // In case a configuration was also loaded (bootrom will recompute the checksum)
    HbiWriteReg(0x1F2, 0x0000);

    // Save firmware to flash
    HbiWriteReg(0x032, 0x0004);
    HbiWriteReg(0x006, 0x0001);
    if (BusySpinWait() == FALSE) {
        sprintf(buffer, "ERROR");
        return;
    }

    status = HbiReadReg(0x034) & 0x000F;
    if (status != 0) {
        fprintf(stderr, "Error - SaveFirmware2Flash(): Can't save firmware to flash (0x%02X)\n", status);
        sprintf(buffer, "ERROR");
    } else {
        sprintf(buffer, "OK");
    }
}

/**********************************************************************
 * Save configuration to flash
 * ********************************************************************/
void SaveConfig2Flash(char * buffer, unsigned char index) {
    hbi_status_t status;

    if (IsFirmwareRunning() == FALSE) {
        // Initialise the flash
        if (InitFlash() == FALSE) {
            sprintf(buffer, "ERROR");
            return;
        }

        // In case a configuration was also loaded (bootrom will recompute the checksum)
        HbiWriteReg(0x1F2, 0x0000);

        // Save configuration to flash
        HbiWriteReg(0x032, 0x0007);
        HbiWriteReg(0x034, index);
        HbiWriteReg(0x006, 0x0001);
    } else {
        // Save configuration to flash
        HbiWriteReg(0x032, 0x8002);
        HbiWriteReg(0x034, index);
        HbiWriteReg(0x006, 0x0004);

        // An extra delay is required after a software semaphore flag and before BusySpinWait()
        usleep(10000);
    }

    if (BusySpinWait() == FALSE) {
        sprintf(buffer, "ERROR");
        return;
    }

    status = HbiReadReg(0x034) & 0x000F;
    if (status != 0) {
        fprintf(stderr, "Error - SaveConfig2Flash(): Can't save configuration to flash (0x%02X)\n", status);
        sprintf(buffer, "ERROR");
    } else {
        sprintf(buffer, "OK");
    }
}

/**********************************************************************
 * Load a firmware and configuration from flash
 * ********************************************************************/
void LoadFwfromFlash(char * buffer, unsigned char index) {
    hbi_status_t status;

    // Initialise the flash (Assumes that the firmware is stopped)
    if (InitFlash() == FALSE) {
        sprintf(buffer, "ERROR");
        return;
    }

    // Load firmware from flash
    HbiWriteReg(0x032, 0x0002);
    HbiWriteReg(0x034, index);
    HbiWriteReg(0x006, 0x0001);

    if (BusySpinWait() == FALSE) {
        sprintf(buffer, "ERROR");
        return;
    }

    status = HbiReadReg(0x034) & 0x000F;
    if (status != 0) {
        fprintf(stderr, "Error - LoadFwfromFlash(): Can't load the firmware from flash (0x%02X)\n", status);
        sprintf(buffer, "ERROR");
    } else {
        sprintf(buffer, "OK");
    }
}

/**********************************************************************
 * Decode the command and return the answer
 * ********************************************************************/
void ParseCmd(char * header, char * cmdAns, unsigned short cmdLen) {
    char buffer[BUFFER_SZ];
    unsigned int data;
    char temp[5];

    // 16b read
    if (strcmp("RD", header) == 0) {
        cmdAns[cmdLen] = 0;
        sprintf(buffer, "%04X", HbiReadReg(strtol(cmdAns, NULL, 16)));

    // 16b write
    } else if (strcmp("WR", header) == 0) {
        cmdAns[cmdLen] = 0;
        data = strtol(cmdAns + 3, NULL, 16);
        cmdAns[3] = 0;
        HbiWriteReg(strtol(cmdAns, NULL, 16), data);
        sprintf(buffer, "OK");

    // Buffer read
    } else if (strcmp("BR", header) == 0) {
        cmdAns[cmdLen] = 0;
        data = strtol(cmdAns + 3, NULL, 16);
        cmdAns[3] = 0;
        HbiReadBuffS(strtol(cmdAns, NULL, 16), buffer, data * 2);

    // Buffer write
    } else if (strcmp("BW", header) == 0) {
        strncpy(temp, cmdAns, 3);
        HbiWriteBuffS(strtol(temp, NULL, 16), cmdAns + 3, (cmdLen - 3) / 2);
        sprintf(buffer, "OK");

    // Hardware buffer read (page 255 access)
    } else if (strcmp("HR", header) == 0) {
        cmdAns[cmdLen] = 0;
        data = strtol(cmdAns + 8, NULL, 16);
        cmdAns[8] = 0;
        HbiHwReadBuffS(strtol(cmdAns, NULL, 16), buffer, data);

    // Firmware loading
    } else if ((strcmp("FA", header) == 0) || (strcmp("FB", header) == 0) || (strcmp("FC", header) == 0)) {
        cmdAns[cmdLen] = 0;
        LoadFirmware(header, cmdAns);
        strcpy(buffer, cmdAns);

    // Erase flash
    } else if (strcmp("ER", header) == 0) {
        EraseFlash(buffer);

    // Save firmware to flash
    } else if (strcmp("SF", header) == 0) {
        SaveFirmware2Flash(buffer);

    // Save configuration to flash
    } else if (strcmp("SC", header) == 0) {
        cmdAns[cmdLen] = 0;
        SaveConfig2Flash(buffer, strtol(cmdAns, NULL, 16));

    // Load a firmware and configuration from flash
    } else if (strcmp("LF", header) == 0) {
        cmdAns[cmdLen] = 0;
        LoadFwfromFlash(buffer, strtol(cmdAns, NULL, 16));

    } else {
        fprintf(stderr, "Error - ParseCmd(): Unknown header (%s)\n", header);
        sprintf(buffer, "ERROR");
    }

    sprintf(cmdAns, "ANS%04X%s", strlen(buffer), buffer);
}

/**********************************************************************
 * Main
 * ********************************************************************/
int main(int argc, char const *argv[]) {
    int serverFd, newSocket, nbRead, opt = 1;
    struct sockaddr_in address;
    int addrLen = sizeof(address);
    unsigned short writeIndex, readIndex, cmdLen, pcn, variant;
    char buffer[BUFFER_SZ];
    char debugBuffer[BUFFER_SZ + 1];
    char str[INET_ADDRSTRLEN];
    char circBuffer[C_BUFFER_SZ];
    char header[HEADER_SZ + 1];
    message_type waitType;
    hbi_status_t status;
    hbi_dev_cfg_t devConfig;
    user_buffer_t buf[2];

    printf("\nWelcome to MiTuner Bridge server, use CTRL+C to exit\n");

    // Intercept CTRL+C as it's the only way to quit this program
    signal(SIGINT, sigintHandler);

    // Open the HBI driver
    devConfig.deviceId = DEVICE_ID;
    devConfig.pDevName = NULL;

    status = HBI_open(&gHandle, &devConfig);
    if(status != HBI_STATUS_SUCCESS) {
        fprintf(stderr, "Error - main(): HBI_open failed (%d)\n", status);
        return -1;
    }

    // Report the DUT parameters
    HbiHwReadBuffS(0x7FFFBC58, buffer, 4);
    printf("Currently connected to:\n\tHW: ZL380%02d\n", strtol(&buffer[6], NULL, 16));
    pcn = HbiReadReg(0x022);
    if (pcn > 38000) {
        variant = (HbiReadReg(0x028) & 0x7F00) >> 8;
        printf("\tFW: ZLS%d.%d\n\n", pcn, variant);
    } else {
        printf("\tFW: Bootrom\n\n");
    }

    // Creating socket file descriptor
    if ((serverFd = socket(AF_INET, SOCK_STREAM, 0)) == 0) {
        fprintf(stderr, "Error - main(): socket() failed\n");
        exit(EXIT_FAILURE);
    }

    // Forcefully attaching socket to the port
    if (setsockopt(serverFd, SOL_SOCKET, SO_REUSEADDR | SO_REUSEPORT, &opt, sizeof(opt))) {
        fprintf(stderr, "Error - main(): setsockopt() failed\n");
        exit(EXIT_FAILURE);
    }
    address.sin_family = AF_INET;
    address.sin_addr.s_addr = INADDR_ANY;
    address.sin_port = htons(PORT);

    // Forcefully attaching socket to the port
    if (bind(serverFd, (struct sockaddr *)&address, sizeof(address)) < 0) {
        fprintf(stderr, "Error - main(): bind() failed\n");
        exit(EXIT_FAILURE);
    }

    if (listen(serverFd, 3) < 0) {
        fprintf(stderr, "Error - main(): listen() failed\n");
        exit(EXIT_FAILURE);
    }

    while (TRUE) {
        if ((newSocket = accept(serverFd, (struct sockaddr *)&address, (socklen_t*)&addrLen)) < 0) {
            fprintf(stderr, "Error - main(): accept() failed\n");
            exit(EXIT_FAILURE);
        }
#ifdef ARPA_AVAILABLE
        printf("Incoming connection from %s\n", inet_ntop(AF_INET, &(address.sin_addr), str, INET_ADDRSTRLEN));
#else
        printf("Incoming connection\n");
#endif
        writeIndex = 0;
        readIndex = 0;
        gCharCount = 0;
        waitType = HEADER;

        while (TRUE) {
            nbRead = read(newSocket, buffer, BUFFER_SZ);

            // Print the buffers for debug
            if (nbRead > 0) {
                strncpy(debugBuffer, buffer, nbRead);
                debugBuffer[nbRead] = 0;
            }

            if (nbRead == 0) {
                // Client got disconnected
                break;
            } else {
                // Add the incoming data to the circular buffer
                StoreBuffer(circBuffer, &writeIndex, buffer, nbRead);
                if ((waitType == HEADER) && (gCharCount >= HEADER_SZ)) {
                    GetBuffer(circBuffer, &readIndex, header, HEADER_SZ);

                    // Terminate the string and extract the message size (skip the first 2 chars)
                    header[HEADER_SZ] = 0;
                    cmdLen = strtol(header + 2, NULL, 16);

                    // Now limit the header to the message type
                    header[2] = 0;

                    // Expect a command next
                    waitType = COMMAND;
                }
                if ((waitType == COMMAND) && (gCharCount >= cmdLen)) {
                    GetBuffer(circBuffer, &readIndex, buffer, cmdLen);

                    // Parse the query and return the answer in "buffer"
                    ParseCmd(header, buffer, cmdLen);
                    send(newSocket, buffer, strlen(buffer), 0);

                    waitType = HEADER;
                }
            }
        }
        printf("Client has been disconnected\n");
    }

    return 0;
}
